
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_motors_matrix.c
  * @author     baiyang
  * @date       2023-2-1
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mb_motors_matrix.h"

#include <vehicle/vehicle_type.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
/// mb_motors_ops
static void init(mb_motors_t motors, enum mb_motor_frame_class frame_class, enum mb_motor_frame_type frame_type);
static void set_frame_class_and_type(mb_motors_t motors, enum mb_motor_frame_class frame_class, enum mb_motor_frame_type frame_type);
static void set_update_rate(mb_motors_t motors, uint16_t speed_hz);
static uint32_t get_motor_mask(mb_motors_t motors);
static uint8_t get_lost_motor(mb_motors_const_t motors);
static float get_roll_factor(mb_motors_t motors, uint8_t i);
static float get_pitch_factor(mb_motors_t motors, uint8_t i);
static const char* _get_frame_string(mb_motors_const_t motors);
static const char* get_type_string(mb_motors_const_t motors);
static void _output_test_seq(mb_motors_t motors, uint8_t motor_seq, int16_t pwm);
static void output_armed_stabilizing(mb_motors_t motors);
static inline float boost_ratio(mb_motors_t motors, float boost_value, float normal_value);

///mb_motors_multicopter_ops
static void output_to_motors(mb_motors_t motors);
static void disable_yaw_torque(mb_motors_t motors);
static void thrust_compensation(mb_motors_t motors);
/*----------------------------------variable----------------------------------*/
static struct mb_motors_ops motors_ops = {.arming_checks = NULL,
                                          .set_roll_pitch = NULL,
                                          .get_throttle_hover = NULL,
                                          .get_lost_motor = get_lost_motor,
                                          .set_update_rate = set_update_rate,
                                          .init = init,
                                          .set_frame_class_and_type = set_frame_class_and_type,
                                          .output = NULL,
                                          .output_min = NULL,
                                          .get_motor_mask = get_motor_mask,
                                          .get_roll_factor = get_roll_factor,
                                          .get_pitch_factor = get_pitch_factor,
                                          .is_motor_enabled = NULL,
                                          .init_targets_on_arming = NULL,
                                          .rc_write = NULL,
                                          .Log_Write = NULL,
                                          .output_armed_stabilizing = output_armed_stabilizing,
                                          .rc_write_angle = NULL,
                                          .rc_set_freq = NULL,
                                          .update_throttle_filter = NULL,
                                          .save_params_on_disarm = NULL,
                                          ._get_frame_string = _get_frame_string,
                                          .get_type_string = get_type_string,
                                          ._output_test_seq = _output_test_seq,
                                          .assign_param     = NULL};

static struct mb_motors_multicopter_ops multicopter_ops = {.output_motor_mask = NULL,
                                                           .disable_yaw_torque = disable_yaw_torque,
                                                           .output_to_motors = output_to_motors,
                                                           .get_current_limit_max_throttle = NULL,
                                                           .thrust_compensation = thrust_compensation,
                                                           .output_boost_throttle = NULL,
                                                           .output_rpyt = NULL};

static struct mb_motors_matrix_ops matrix_ops;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// Constructor
void mb_motors_matrix_ctor(mb_motors_mat_t motors_matrix, mb_motors_mat_ops_t ops_mat, mb_motors_mc_ops_t ops_mc, mb_motors_ops_t ops, uint16_t speed_hz)
{
    if (ops_mat == NULL) {
        ops_mat = &matrix_ops;
    }

    if (ops_mc == NULL) {
        ops_mc = &multicopter_ops;
    }

    if (ops == NULL) {
        ops = &motors_ops;
    }

    mb_motors_bind_motors_function(ops, &motors_ops);
    mb_motors_bind_motors_mc_function(ops_mc, &multicopter_ops);

    mb_motors_multicopter_ctor(&motors_matrix->motors_mc, ops_mc, ops, speed_hz);

    motors_matrix->ops_mat = ops_mat;

    motors_matrix->_frame_class_string = "";
    motors_matrix->_frame_type_string  = "";
}

// output_test_num - spin a motor connected to the specified output channel
//  (should only be performed during testing)
//  If a motor output channel is remapped, the mapped channel is used.
//  Returns true if motor output is set, false otherwise
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
bool mb_motors_output_test_num(mb_motors_t motors, uint8_t output_channel, int16_t pwm)
{
    if (!mb_motors_armed(motors)) {
        return false;
    }

    // Is channel in supported range?
    if (output_channel > MB_MOTORS_MAX_NUM_MOTORS - 1) {
        return false;
    }

    // Is motor enabled?
    if (!((mb_motors_mc_t)motors)->motor_enabled[output_channel]) {
        return false;
    }

    mb_motors_rc_write(motors, output_channel, pwm); // output
    return true;
}

// add_motor using raw roll, pitch, throttle and yaw factors
// default, float throttle_factor = 1.0f
void mb_motors_add_motor_raw(mb_motors_t motors, int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order, float throttle_factor)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    if (mb_motors_initialised_ok(motors)) {
        // do not allow motors to be set if the current frame type has init correctly
        return;
    }

    // ensure valid motor number is provided
    if (motor_num >= 0 && motor_num < MB_MOTORS_MAX_NUM_MOTORS) {

        // enable motor
        motors_mc->motor_enabled[motor_num] = true;

        // set roll, pitch, yaw and throttle factors
        motors_mat->_roll_factor[motor_num] = roll_fac;
        motors_mat->_pitch_factor[motor_num] = pitch_fac;
        motors_mat->_yaw_factor[motor_num] = yaw_fac;
        motors_mat->_throttle_factor[motor_num] = throttle_factor;

        // set order that motor appears in test
        motors_mat->_test_order[motor_num] = testing_order;

        // call parent class method
        mb_motors_add_motor_num(motors, motor_num);
    }
}

// method to add many motors specified in a structure:
void mb_motors_add_motors(mb_motors_t motors, const struct MotorDef *motors_def, uint8_t num_motors)
{
    for (uint8_t i=0; i<num_motors; i++) {
        mb_motors_add_motor(motors, i, motors_def[i].angle_degrees, motors_def[i].yaw_factor, motors_def[i].testing_order);
    }
}

void mb_motors_add_motors_raw(mb_motors_t motors, const struct MotorDefRaw *motors_def_raw, uint8_t num_motors)
{
    for (uint8_t i=0; i<num_motors; i++) {
        mb_motors_add_motor_raw(motors, i, motors_def_raw[i].roll_fac, motors_def_raw[i].pitch_fac, motors_def_raw[i].yaw_fac, motors_def_raw[i].testing_order, 1.0f);
    }
}

// add_motor using just position and yaw_factor (or prop direction)
void mb_motors_add_motor(mb_motors_t motors, int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order)
{
    mb_motors_add_motor2(motors, motor_num, angle_degrees, angle_degrees, yaw_factor, testing_order);
}

// remove_motor
void mb_motors_remove_motor(mb_motors_t motors, int8_t motor_num)
{
    // ensure valid motor number is provided
    if (motor_num >= 0 && motor_num < MB_MOTORS_MAX_NUM_MOTORS) {
        // disable the motor, set all factors to zero
        ((mb_motors_mc_t)motors)->motor_enabled[motor_num] = false;
        ((mb_motors_mat_t)motors)->_roll_factor[motor_num] = 0.0f;
        ((mb_motors_mat_t)motors)->_pitch_factor[motor_num] = 0.0f;
        ((mb_motors_mat_t)motors)->_yaw_factor[motor_num] = 0.0f;
        ((mb_motors_mat_t)motors)->_throttle_factor[motor_num] = 0.0f;
    }
}

// add_motor using separate roll and pitch factors (for asymmetrical frames) and prop direction
void mb_motors_add_motor2(mb_motors_t motors, int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order)
{
    mb_motors_add_motor_raw(
        motors,
        motor_num,
        cosf(radians(roll_factor_in_degrees + 90)),
        cosf(radians(pitch_factor_in_degrees)),
        yaw_factor,
        testing_order,
        1.0f);
}

// check for failed motor
//   should be run immediately after output_armed_stabilizing
//   first argument is the sum of:
//      a) throttle_thrust_best_rpy : throttle level (from 0 to 1) providing maximum roll, pitch and yaw range without climbing
//      b) thr_adj: the difference between the pilot's desired throttle and throttle_thrust_best_rpy
//   records filtered motor output values in _thrust_rpyt_out_filt array
//   sets thrust_balanced to true if motors are balanced, false if a motor failure is detected
//   sets _motor_lost_index to index of failed motor
void mb_motors_check_for_failed_motor(mb_motors_t motors, float throttle_thrust_best_plus_adj)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    // record filtered and scaled thrust output for motor loss monitoring purposes
    float alpha = motors->_dt / (motors->_dt + 0.5f);
    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            motors_mat->_thrust_rpyt_out_filt[i] += alpha * (motors_mat->_thrust_rpyt_out[i] - motors_mat->_thrust_rpyt_out_filt[i]);
        }
    }

    float rpyt_high = 0.0f;
    float rpyt_sum = 0.0f;
    uint8_t number_motors = 0.0f;
    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            number_motors += 1;
            rpyt_sum += motors_mat->_thrust_rpyt_out_filt[i];
            // record highest filtered thrust command
            if (motors_mat->_thrust_rpyt_out_filt[i] > rpyt_high) {
                rpyt_high = motors_mat->_thrust_rpyt_out_filt[i];
                // hold motor lost index constant while thrust boost is active
                if (!motors->_thrust_boost) {
                    motors_mat->_motor_lost_index = i;
                }
            }
        }
    }

    float thrust_balance = 1.0f;
    if (rpyt_sum > 0.1f) {
        thrust_balance = rpyt_high * number_motors / rpyt_sum;
    }
    // ensure thrust balance does not activate for multirotors with less than 6 motors
    if (number_motors >= 6 && thrust_balance >= 1.5f && motors->_thrust_balanced) {
        motors->_thrust_balanced = false;
    }
    if (thrust_balance <= 1.25f && !motors->_thrust_balanced) {
        motors->_thrust_balanced = true;
    }

    // check to see if thrust boost is using more throttle than _throttle_thrust_max
    if ((motors_mc->_throttle_thrust_max * mb_motors_get_compensation_gain(motors) > throttle_thrust_best_plus_adj) && (rpyt_high < 0.9f) && motors->_thrust_balanced) {
        motors->_thrust_boost = false;
    }
}

// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
// normalizes throttle factors so max value is 1 and no value is less than 0
void mb_motors_normalise_rpy_factors(mb_motors_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    float roll_fac = 0.0f;
    float pitch_fac = 0.0f;
    float yaw_fac = 0.0f;
    float throttle_fac = 0.0f;

    // find maximum roll, pitch and yaw factors
    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            roll_fac = MAX(roll_fac,fabsf(motors_mat->_roll_factor[i]));
            pitch_fac = MAX(pitch_fac,fabsf(motors_mat->_pitch_factor[i]));
            yaw_fac = MAX(yaw_fac,fabsf(motors_mat->_yaw_factor[i]));
            throttle_fac = MAX(throttle_fac,MAX(0.0f,motors_mat->_throttle_factor[i]));
        }
    }

    // scale factors back to -0.5 to +0.5 for each axis
    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            if (!math_flt_zero(roll_fac)) {
                motors_mat->_roll_factor[i] = 0.5f * motors_mat->_roll_factor[i] / roll_fac;
            }
            if (!math_flt_zero(pitch_fac)) {
                motors_mat->_pitch_factor[i] = 0.5f * motors_mat->_pitch_factor[i] / pitch_fac;
            }
            if (!math_flt_zero(yaw_fac)) {
                motors_mat->_yaw_factor[i] = 0.5f * motors_mat->_yaw_factor[i] / yaw_fac;
            }
            if (!math_flt_zero(throttle_fac)) {
                motors_mat->_throttle_factor[i] = MAX(0.0f,motors_mat->_throttle_factor[i] / throttle_fac);
            }
        }
    }
}

void mb_motors_bind_motors_mc_function(mb_motors_mc_ops_t ops_frontend, mb_motors_mc_ops_t ops_motors_matrix)
{
    if (ops_frontend == ops_motors_matrix) {
        return;
    }

    if (ops_frontend->output_motor_mask == NULL && ops_motors_matrix->output_motor_mask != NULL) {
        ops_frontend->output_motor_mask = ops_motors_matrix->output_motor_mask;
    }

    if (ops_frontend->disable_yaw_torque == NULL && ops_motors_matrix->disable_yaw_torque != NULL) {
        ops_frontend->disable_yaw_torque = ops_motors_matrix->disable_yaw_torque;
    }

    if (ops_frontend->output_to_motors == NULL && ops_motors_matrix->output_to_motors != NULL) {
        ops_frontend->output_to_motors = ops_motors_matrix->output_to_motors;
    }

    if (ops_frontend->get_current_limit_max_throttle == NULL && ops_motors_matrix->get_current_limit_max_throttle != NULL) {
        ops_frontend->get_current_limit_max_throttle = ops_motors_matrix->get_current_limit_max_throttle;
    }

    if (ops_frontend->thrust_compensation == NULL && ops_motors_matrix->thrust_compensation != NULL) {
        ops_frontend->thrust_compensation = ops_motors_matrix->thrust_compensation;
    }

    if (ops_frontend->output_boost_throttle == NULL && ops_motors_matrix->output_boost_throttle != NULL) {
        ops_frontend->output_boost_throttle = ops_motors_matrix->output_boost_throttle;
    }

    if (ops_frontend->output_rpyt == NULL && ops_motors_matrix->output_rpyt != NULL) {
        ops_frontend->output_rpyt = ops_motors_matrix->output_rpyt;
    }
}

/// 
#if MB_MOTORS_FRAME_QUAD_ENABLED
static bool setup_quad_matrix(mb_motors_t motors, enum mb_motor_frame_type frame_type)
{
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    motors_mat->_frame_class_string = "QUAD";
    motors->_mav_type = MAV_TYPE_QUADROTOR;

    switch (frame_type) {
    case MB_MOTOR_FRAME_TYPE_PLUS: {
        motors_mat->_frame_type_string = "PLUS";
        static const struct MotorDef motors_def[] = {
            {  90, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
            { -90, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
            {   0, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
            { 180, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_X: {
        motors_mat->_frame_type_string = "X";
        static const struct MotorDef motors_def[] = {
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
#if MB_BUILD_TYPE(MICROBEE_BUILD_Plane)
    case MB_MOTOR_FRAME_TYPE_NYT_PLUS: {
        motors_mat->_frame_type_string = "NYT_PLUS";
        static const struct MotorDef motors_def[] = {
            {  90, 0,  2 },
            { -90, 0,  4 },
            {   0, 0,  1 },
            { 180, 0,  3 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_NYT_X: {
        motors_mat->_frame_type_string = "NYT_X";
        static const struct MotorDef motors_def[] = {
            {   45, 0,  1 },
            { -135, 0,  3 },
            {  -45, 0,  4 },
            {  135, 0,  2 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
#endif //APM_BUILD_TYPE(APM_BUILD_ArduPlane)
    case MB_MOTOR_FRAME_TYPE_BF_X: {
        // betaflight quad X order
        // see: https://fpvfrenzy.com/betaflight-motor-order/
        motors_mat->_frame_type_string = "BF_X";
        static const struct MotorDef motors_def[] = {
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  2 },
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  4 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_BF_X_REV: {
        // betaflight quad X order, reversed motors
        motors_mat->_frame_type_string = "X_REV";
        static const struct MotorDef motors_def[] = {
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_DJI_X: {
        // DJI quad X order
        // see https://forum44.djicdn.com/data/attachment/forum/201711/26/172348bppvtt1ot1nrtp5j.jpg
        motors_mat->_frame_type_string = "DJI_X";
        static const struct MotorDef motors_def[] = {
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_CW_X: {
        // "clockwise X" motor order. Motors are ordered clockwise from front right
        // matching test order
        motors_mat->_frame_type_string = "CW_X";
        static const struct MotorDef motors_def[] = {
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_V: {
        motors_mat->_frame_type_string = "V";
        static const struct MotorDef motors_def[] = {
            {   45,   0.7981f,   1 },
            { -135,   1.0000f,   3 },
            {  -45,  -0.7981f,   4 },
            {  135,  -1.0000f,   2 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_H: {
        // H frame set-up - same as X but motors spin in opposite directiSons
        motors_mat->_frame_type_string = "H";
        static const struct MotorDef motors_def[] = {
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_VTAIL: {
        /*
            Tested with: Lynxmotion Hunter Vtail 400
            - inverted rear outward blowing motors (at a 40 degree angle)
            - should also work with non-inverted rear outward blowing motors
            - no roll in rear motors
            - no yaw in front motors
            - should fly like some mix between a tricopter and X Quadcopter

            Roll control comes only from the front motors, Yaw control only from the rear motors.
            Roll & Pitch factor is measured by the angle away from the top of the forward axis to each arm.

            Note: if we want the front motors to help with yaw,
                motors 1's yaw factor should be changed to sin(radians(40)).  Where "40" is the vtail angle
                motors 3's yaw factor should be changed to -sin(radians(40))
        */
        motors_mat->_frame_type_string = "VTAIL";
        mb_motors_add_motor2(motors, MB_MOTORS_MOT_1, 60, 60, 0, 1);
        mb_motors_add_motor2(motors, MB_MOTORS_MOT_2, 0, -160, MB_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
        mb_motors_add_motor2(motors, MB_MOTORS_MOT_3, -60, -60, 0, 4);
        mb_motors_add_motor2(motors, MB_MOTORS_MOT_4, 0, 160, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
        break;
    }
    case MB_MOTOR_FRAME_TYPE_ATAIL:
        /*
            The A-Shaped VTail is the exact same as a V-Shaped VTail, with one difference:
            - The Yaw factors are reversed, because the rear motors are facing different directions

            With V-Shaped VTails, the props make a V-Shape when spinning, but with
            A-Shaped VTails, the props make an A-Shape when spinning.
            - Rear thrust on a V-Shaped V-Tail Quad is outward
            - Rear thrust on an A-Shaped V-Tail Quad is inward

            Still functions the same as the V-Shaped VTail mixing below:
            - Yaw control is entirely in the rear motors
            - Roll is is entirely in the front motors
        */
        motors_mat->_frame_type_string = "ATAIL";
        mb_motors_add_motor2(motors, MB_MOTORS_MOT_1, 60, 60, 0, 1);
        mb_motors_add_motor2(motors, MB_MOTORS_MOT_2, 0, -160, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
        mb_motors_add_motor2(motors, MB_MOTORS_MOT_3, -60, -60, 0, 4);
        mb_motors_add_motor2(motors, MB_MOTORS_MOT_4, 0, 160, MB_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
        break;
    case MB_MOTOR_FRAME_TYPE_PLUSREV: {
        // plus with reversed motor directions
        motors_mat->_frame_type_string = "PLUSREV";
        static const struct MotorDef motors_def[] = {
            { 90, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  2 },
            { -90, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  4 },
            { 0, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
            { 180, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_Y4:
        motors_mat->_frame_type_string = "Y4";
        // Y4 motor definition with right front CCW, left front CW
        static const struct MotorDefRaw motors_def_raw[] = {
            { -1.0f,  1.000f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
            {  0.0f, -1.000f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  2 },
            {  0.0f, -1.000f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
            {  1.0f,  1.000f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  4 },
        };
        mb_motors_add_motors_raw(motors, motors_def_raw, ARRAY_SIZE(motors_def_raw));
        break;
    default:
        // quad frame class does not support this frame type
        motors_mat->_frame_type_string = "UNSUPPORTED";
        return false;
    }
    return true;
}
#endif

#if MB_MOTORS_FRAME_HEXA_ENABLED
static bool setup_hexa_matrix(mb_motors_t motors, enum mb_motor_frame_type frame_type)
{
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    motors_mat->_frame_class_string = "HEXA";
    motors->_mav_type = MAV_TYPE_HEXAROTOR;
    switch (frame_type) {
    case MB_MOTOR_FRAME_TYPE_PLUS: {
        motors_mat->_frame_type_string = "PLUS";
        static const struct MotorDef motors_def[] = {
            {    0, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
            {  180, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
            { -120, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   5 },
            {   60, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
            {  -60, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  6 },
            {  120, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_X: {
        motors_mat->_frame_type_string = "X";
        static const struct MotorDef motors_def[] = {
            {   90, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
            {  -90, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
            {  -30, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
            {  150, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
            {   30, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
            { -150, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_H: {
        // H is same as X except middle motors are closer to center
        motors_mat->_frame_type_string = "H";
        static const struct MotorDefRaw motors_def_raw[] = {
            { -1.0f, 0.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CW, 2 },
            { 1.0f, 0.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
            { 1.0f, 1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CW, 6 },
            { -1.0f, -1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
            { -1.0f, 1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
            { 1.0f, -1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CW, 4 },
        };
        mb_motors_add_motors_raw(motors, motors_def_raw, ARRAY_SIZE(motors_def_raw));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_DJI_X: {
        motors_mat->_frame_type_string = "DJI_X";
        static const struct MotorDef motors_def[] = {
            {   30, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
            {  -30, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
            {  -90, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
            { -150, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
            {  150, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
            {   90, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_CW_X: {
        motors_mat->_frame_type_string = "CW_X";
        static const struct MotorDef motors_def[] = {
            {   30, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
            {   90, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
            {  150, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
            { -150, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
            {  -90, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
            {  -30, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    default:
        // hexa frame class does not support this frame type
        motors_mat->_frame_type_string = "UNSUPPORTED";
        return false;
    } //hexa
    return true;
}
#endif ////MB_MOTORS_FRAME_HEXA_ENABLED

#if MB_MOTORS_FRAME_OCTA_ENABLED
static bool setup_octa_matrix(mb_motors_t motors, enum mb_motor_frame_type frame_type)
{
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    motors_mat->_frame_class_string = "OCTA";
    motors->_mav_type = MAV_TYPE_OCTOROTOR;
    switch (frame_type) {
    case MB_MOTOR_FRAME_TYPE_PLUS: {
        motors_mat->_frame_type_string = "PLUS";
        static const struct MotorDef motors_def[] = {
            {    0, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
            {  180, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   5 },
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  8 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  6 },
            {  -90, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   7 },
            {   90, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
        };

        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_X: {
        motors_mat->_frame_type_string = "X";
        static const struct MotorDef motors_def[] = {
            {   22.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
            { -157.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CW,   5 },
            {   67.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
            {  157.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
            {  -22.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  8 },
            { -112.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  6 },
            {  -67.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CW,   7 },
            {  112.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_V: {
        motors_mat->_frame_type_string = "V";
        static const struct MotorDefRaw motors_def_raw[] = {
            {  0.83f,  0.34f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  7 },
            { -0.67f, -0.32f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  3 },
            {  0.67f, -0.32f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
            { -0.50f, -1.00f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
            {  1.00f,  1.00f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
            { -0.83f,  0.34f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
            { -1.00f,  1.00f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  1 },
            {  0.50f, -1.00f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  5 },
        };
        mb_motors_add_motors_raw(motors, motors_def_raw, ARRAY_SIZE(motors_def_raw));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_H: {
        motors_mat->_frame_type_string = "H";
        static const struct MotorDefRaw motors_def_raw[] = {
            { -1.0f,    1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  1 },
            {  1.0f,   -1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  5 },
            { -1.0f,  0.333f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
            { -1.0f,   -1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
            {  1.0f,    1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
            {  1.0f, -0.333f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
            {  1.0f,  0.333f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  7 },
            { -1.0f, -0.333f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  3 },
        };
        mb_motors_add_motors_raw(motors, motors_def_raw, ARRAY_SIZE(motors_def_raw));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_I: {
        motors_mat->_frame_type_string = "I";
        static const struct MotorDefRaw motors_def_raw[] = {
            { 0.333f, -1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   5 },
            { -0.333f,  1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  1 },
            {    1.0f, -1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
            {  0.333f,  1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
            { -0.333f, -1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
            {   -1.0f,  1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
            {   -1.0f, -1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  3 },
            {    1.0f,  1.0f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  7 },
        };
        mb_motors_add_motors_raw(motors, motors_def_raw, ARRAY_SIZE(motors_def_raw));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_DJI_X: {
        motors_mat->_frame_type_string = "DJI_X";
        static const struct MotorDef motors_def[] = {
            {   22.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
            {  -22.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CW,   8 },
            {  -67.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  7 },
            { -112.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
            { -157.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
            {  157.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
            {  112.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
            {   67.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_CW_X: {
        motors_mat->_frame_type_string = "CW_X";
        static const struct MotorDef motors_def[] = {
            {   22.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
            {   67.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
            {  112.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
            {  157.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
            { -157.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
            { -112.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
            {  -67.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  7 },
            {  -22.5f,  MB_MOTORS_MATRIX_YAW_FACTOR_CW,   8 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    default:
        // octa frame class does not support this frame type
        motors_mat->_frame_type_string = "UNSUPPORTED";
        return false;
    } // octa frame type
    return true;
}
#endif //MB_MOTORS_FRAME_OCTA_ENABLED

#if MB_MOTORS_FRAME_OCTAQUAD_ENABLED
static bool setup_octaquad_matrix(mb_motors_t motors, enum mb_motor_frame_type frame_type)
{
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    motors->_mav_type = MAV_TYPE_OCTOROTOR;
    motors_mat->_frame_class_string = "OCTAQUAD";
    switch (frame_type) {
    case MB_MOTOR_FRAME_TYPE_PLUS: {
        motors_mat->_frame_type_string = "PLUS";
        static const struct MotorDef motors_def[] = {
            {   0, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
            { -90, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   7 },
            { 180, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
            {  90, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
            { -90, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  8 },
            {   0, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
            {  90, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
            { 180, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_X: {
        motors_mat->_frame_type_string = "X";
        static const struct MotorDef motors_def[] = {
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   7 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  8 },
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_V: {
        motors_mat->_frame_type_string = "V";
        static const struct MotorDef motors_def[] = {
            {   45,   0.7981f,  1 },
            {  -45,  -0.7981f,  7 },
            { -135,   1.0000f,  5 },
            {  135,  -1.0000f,  3 },
            {  -45,   0.7981f,  8 },
            {   45,  -0.7981f,  2 },
            {  135,   1.0000f,  4 },
            { -135,  -1.0000f,  6 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_H: {
        // H frame set-up - same as X but motors spin in opposite directions
        motors_mat->_frame_type_string = "H";
        static const struct MotorDef motors_def[] = {
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   1 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  7 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   5 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  3 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   8 },
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  2 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   4 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  6 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_CW_X: {
        motors_mat->_frame_type_string = "CW_X";
        static const struct MotorDef motors_def[] = {
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 },
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   2 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   3 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   6 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   7 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  8 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    // BF/X cinelifters using two 4-in-1 ESCs are quite common
    // see: https://fpvfrenzy.com/betaflight-motor-order/
    case MB_MOTOR_FRAME_TYPE_BF_X: {
        motors_mat->_frame_type_string = "BF_X";
        static const struct MotorDef motors_def[] = {
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  3 },
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  7 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  2 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  6 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 8 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_BF_X_REV: {
    // betaflight octa quad X order, reversed motors
        motors_mat->_frame_type_string = "X_REV";
        static const struct MotorDef motors_def[] = {
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  1 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  5 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 7 },
            {  135, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  4 },
            {   45, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
            { -135, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
            {  -45, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  8 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    default:
        // octaquad frame class does not support this frame type
        motors_mat->_frame_type_string = "UNSUPPORTED";
        return false;
    } //octaquad
    return true;
}
#endif // MB_MOTORS_FRAME_OCTAQUAD_ENABLED

#if MB_MOTORS_FRAME_DODECAHEXA_ENABLED
static bool setup_dodecahexa_matrix(mb_motors_t motors, enum mb_motor_frame_type frame_type)
{
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    motors->_mav_type = MAV_TYPE_DODECAROTOR;
    motors_mat->_frame_class_string = "DODECAHEXA";
    switch (frame_type) {
    case MB_MOTOR_FRAME_TYPE_PLUS: {
        motors_mat->_frame_type_string = "PLUS";
        static const struct MotorDef motors_def[] = {
            {    0, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  1 }, // forward-top
            {    0, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   2 }, // forward-bottom
            {   60, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   3 }, // forward-right-top
            {   60, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  4 }, // forward-right-bottom
            {  120, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  5 }, // back-right-top
            {  120, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   6 }, // back-right-bottom
            {  180, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   7 }, // back-top
            {  180, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  8 }, // back-bottom
            { -120, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  9 }, // back-left-top
            { -120, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   10 }, // back-left-bottom
            {  -60, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   11 }, // forward-left-top
            {  -60, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  12 }, // forward-left-bottom
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_X: {
        motors_mat->_frame_type_string = "X";
        static const struct MotorDef motors_def[] = {
            {   30, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   1 }, // forward-right-top
            {   30, MB_MOTORS_MATRIX_YAW_FACTOR_CW,    2 }, // forward-right-bottom
            {   90, MB_MOTORS_MATRIX_YAW_FACTOR_CW,    3 }, // right-top
            {   90, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   4 }, // right-bottom
            {  150, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   5 }, // back-right-top
            {  150, MB_MOTORS_MATRIX_YAW_FACTOR_CW,    6 }, // back-right-bottom
            { -150, MB_MOTORS_MATRIX_YAW_FACTOR_CW,    7 }, // back-left-top
            { -150, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   8 }, // back-left-bottom
            {  -90, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   9 }, // left-top
            {  -90, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   10 }, // left-bottom
            {  -30, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   11 }, // forward-left-top
            {  -30, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,  12 }, // forward-left-bottom
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    default:
        // dodeca-hexa frame class does not support this frame type
        motors_mat->_frame_type_string = "UNSUPPORTED";
        return false;
    } //dodecahexa
    return true;
}
#endif //MB_MOTORS_FRAME_DODECAHEXA_ENABLED


#if MB_MOTORS_FRAME_Y6_ENABLED
static bool setup_y6_matrix(mb_motors_t motors, enum mb_motor_frame_type frame_type)
{
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    motors->_mav_type = MAV_TYPE_HEXAROTOR;
    motors_mat->_frame_class_string = "Y6";
    switch (frame_type) {
    case MB_MOTOR_FRAME_TYPE_Y6B: {
        // Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise
        motors_mat->_frame_type_string = "Y6B";
        static const struct MotorDefRaw motors_def_raw[] = {
            { -1.0f,  0.500f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  1 },
            { -1.0f,  0.500f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
            {  0.0f, -1.000f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  3 },
            {  0.0f, -1.000f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 4 },
            {  1.0f,  0.500f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  5 },
            {  1.0f,  0.500f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
        };
        mb_motors_add_motors_raw(motors, motors_def_raw, ARRAY_SIZE(motors_def_raw));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_Y6F: {
        // Y6 motor layout for FireFlyY6
        motors_mat->_frame_type_string = "Y6F";
        static const struct MotorDefRaw motors_def_raw[] = {
            {  0.0f, -1.000f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
            { -1.0f,  0.500f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 1 },
            {  1.0f,  0.500f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 5 },
            {  0.0f, -1.000f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  4 },
            { -1.0f,  0.500f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  2 },
            {  1.0f,  0.500f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  6 },
        };
        mb_motors_add_motors_raw(motors, motors_def_raw, ARRAY_SIZE(motors_def_raw));
        break;
    }
    default: {
        motors_mat->_frame_type_string = "default";
        static const struct MotorDefRaw motors_def_raw[] = {
            { -1.0f,  0.666f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 2 },
            {  1.0f,  0.666f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  5 },
            {  1.0f,  0.666f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 6 },
            {  0.0f, -1.333f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  4 },
            { -1.0f,  0.666f, MB_MOTORS_MATRIX_YAW_FACTOR_CW,  1 },
            {  0.0f, -1.333f, MB_MOTORS_MATRIX_YAW_FACTOR_CCW, 3 },
        };
        mb_motors_add_motors_raw(motors, motors_def_raw, ARRAY_SIZE(motors_def_raw));
        break;
    }
    } //y6
    return true;
}
#endif // MB_MOTORS_FRAME_Y6_ENABLED

#if MB_MOTORS_FRAME_DECA_ENABLED
static bool setup_deca_matrix(mb_motors_t motors, enum mb_motor_frame_type frame_type)
{
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    motors->_mav_type = MAV_TYPE_DECAROTOR;
    motors_mat->_frame_class_string = "DECA";
    switch (frame_type) {
    case MB_MOTOR_FRAME_TYPE_PLUS: {
        motors_mat->_frame_type_string = "PLUS";
        static const struct MotorDef motors_def[] = {
            {    0, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   1 },
            {   36, MB_MOTORS_MATRIX_YAW_FACTOR_CW,    2 },
            {   72, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   3 },
            {  108, MB_MOTORS_MATRIX_YAW_FACTOR_CW,    4 },
            {  144, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   5 },
            {  180, MB_MOTORS_MATRIX_YAW_FACTOR_CW,    6 },
            { -144, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   7 },
            { -108, MB_MOTORS_MATRIX_YAW_FACTOR_CW,    8 },
            {  -72, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   9 },
            {  -36, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   10 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    case MB_MOTOR_FRAME_TYPE_X:
    case MB_MOTOR_FRAME_TYPE_CW_X: {
        motors_mat->_frame_type_string = "X/CW_X";
        static const struct MotorDef motors_def[] = {
            {   18, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   1 },
            {   54, MB_MOTORS_MATRIX_YAW_FACTOR_CW,    2 },
            {   90, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   3 },
            {  126, MB_MOTORS_MATRIX_YAW_FACTOR_CW,    4 },
            {  162, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   5 },
            { -162, MB_MOTORS_MATRIX_YAW_FACTOR_CW,    6 },
            { -126, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   7 },
            {  -90, MB_MOTORS_MATRIX_YAW_FACTOR_CW,    8 },
            {  -54, MB_MOTORS_MATRIX_YAW_FACTOR_CCW,   9 },
            {  -18, MB_MOTORS_MATRIX_YAW_FACTOR_CW,   10 },
        };
        mb_motors_add_motors(motors, motors_def, ARRAY_SIZE(motors_def));
        break;
    }
    default:
        // deca frame class does not support this frame type
        return false;
    } //deca
    return true;
}
#endif // MB_MOTORS_FRAME_DECA_ENABLED

/// 
void mb_motors_SetupMotors(mb_motors_t motors, enum mb_motor_frame_class frame_class, enum mb_motor_frame_type frame_type)
{
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    // remove existing motors
    for (int8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        mb_motors_remove_motor(motors, i);
    }
    mb_motors_set_initialised_ok(motors, false);
    bool success = true;

    switch (frame_class) {
#if MB_MOTORS_FRAME_QUAD_ENABLED
    case MB_MOTOR_FRAME_QUAD:
        success = setup_quad_matrix(motors, frame_type);
        break;  // quad
#endif //MB_MOTORS_FRAME_QUAD_ENABLED
#if MB_MOTORS_FRAME_HEXA_ENABLED
    case MB_MOTOR_FRAME_HEXA:
        success = setup_hexa_matrix(motors, frame_type);
        break;
#endif //MB_MOTORS_FRAME_HEXA_ENABLED
#if MB_MOTORS_FRAME_OCTA_ENABLED
    case MB_MOTOR_FRAME_OCTA:
        success = setup_octa_matrix(motors, frame_type);
        break;
#endif //MB_MOTORS_FRAME_OCTA_ENABLED
#if MB_MOTORS_FRAME_OCTAQUAD_ENABLED
    case MB_MOTOR_FRAME_OCTAQUAD:
        success = setup_octaquad_matrix(motors, frame_type);
        break;
#endif //MB_MOTORS_FRAME_OCTAQUAD_ENABLED
#if MB_MOTORS_FRAME_DODECAHEXA_ENABLED
    case MB_MOTOR_FRAME_DODECAHEXA:
        success = setup_dodecahexa_matrix(motors, frame_type);
        break;
#endif //MB_MOTORS_FRAME_DODECAHEXA_ENABLED
#if MB_MOTORS_FRAME_Y6_ENABLED
    case MB_MOTOR_FRAME_Y6:
        success = setup_y6_matrix(motors, frame_type);
        break;
#endif //MB_MOTORS_FRAME_Y6_ENABLED
#if MB_MOTORS_FRAME_DECA_ENABLED
    case MB_MOTOR_FRAME_DECA:
        success = setup_deca_matrix(motors, frame_type);
        break;
#endif //MB_MOTORS_FRAME_DECA_ENABLED
    default:
        // matrix doesn't support the configured class
        motors_mat->_frame_class_string = "UNSUPPORTED";
        success = false;
        motors->_mav_type = MAV_TYPE_GENERIC;
        break;
    } // switch frame_class

    // normalise factors to magnitude 0.5
    mb_motors_normalise_rpy_factors(motors);

    mb_motors_set_initialised_ok(motors, success);
}

/// mb_motors_ops

// init
static void init(mb_motors_t motors, enum mb_motor_frame_class frame_class, enum mb_motor_frame_type frame_type)
{
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    // record requested frame class and type
    motors_mat->_active_frame_class = frame_class;
    motors_mat->_active_frame_type = frame_type;

    if (frame_class == MB_MOTOR_FRAME_SCRIPTING_MATRIX) {
        // if Scripting frame class, do nothing scripting must call its own dedicated init function
        return;
    }

    // setup the motors
    mb_motors_setup_motors(motors, frame_class, frame_type);

    // enable fast channels or instant pwm
    mb_motors_set_update_rate(motors, motors->_speed_hz);
}

// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
static void set_frame_class_and_type(mb_motors_t motors, enum mb_motor_frame_class frame_class, enum mb_motor_frame_type frame_type)
{
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    // exit immediately if armed or no change
    if (mb_motors_armed(motors) || (frame_class == motors_mat->_active_frame_class && motors_mat->_active_frame_type == frame_type)) {
        return;
    }
    motors_mat->_active_frame_class = frame_class;
    motors_mat->_active_frame_type = frame_type;

    mb_motors_init(motors, frame_class, frame_type);

}

// set update rate to motors - a value in hertz
static void set_update_rate(mb_motors_t motors, uint16_t speed_hz)
{
    // record requested speed
    motors->_speed_hz = speed_hz;

    uint32_t mask = 0;
    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (((mb_motors_mc_t)motors)->motor_enabled[i]) {
            mask |= 1U << i;
        }
    }
    mb_motors_rc_set_freq(motors, mask, motors->_speed_hz);
}

// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
//  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
static uint32_t get_motor_mask(mb_motors_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    uint32_t motor_mask = 0;
    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            motor_mask |= 1U << i;
        }
    }
    uint32_t mask = mb_motors_motor_mask_to_srv_channel_mask(motors, motor_mask);

    // add parent's mask
    mask |= mb_motors_GetMotorMask(motors);

    return mask;
}

// return number of motor that has failed.  Should only be called if get_thrust_boost() returns true
static uint8_t get_lost_motor(mb_motors_const_t motors)
{
    return ((mb_motors_mat_const_t)motors)->_motor_lost_index;
}

// return the roll factor of any motor, this is used for tilt rotors and tail sitters
// using copter motors for forward flight
static float get_roll_factor(mb_motors_t motors, uint8_t i)
{
    return ((mb_motors_mat_const_t)motors)->_roll_factor[i];
}

// return the pitch factor of any motor
static float get_pitch_factor(mb_motors_t motors, uint8_t i)
{
    return ((mb_motors_mat_const_t)motors)->_pitch_factor[i];
}

static const char* _get_frame_string(mb_motors_const_t motors)
{
    return ((mb_motors_mat_const_t)motors)->_frame_class_string;
}

static const char* get_type_string(mb_motors_const_t motors)
{
    return ((mb_motors_mat_const_t)motors)->_frame_type_string;
}

// output_test_seq - spin a motor at the pwm value specified
//  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
static void _output_test_seq(mb_motors_t motors, uint8_t motor_seq, int16_t pwm)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    // loop through all the possible orders spinning any motors that match that description
    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i] && motors_mat->_test_order[i] == motor_seq) {
            // turn on this motor
            mb_motors_rc_write(motors, i, pwm);
        }
    }
}

// helper to return value scaled between boost and normal based on the value of _thrust_boost_ratio
// _thrust_boost_ratio of 1 -> return = boost_value
// _thrust_boost_ratio of 0 -> return = normal_value
static inline float boost_ratio(mb_motors_t motors, float boost_value, float normal_value)
{
    return motors->_thrust_boost_ratio * boost_value + (1.0 - motors->_thrust_boost_ratio) * normal_value;
}

// output_armed - sends commands to the motors
// includes new scaling stability patch
static void output_armed_stabilizing(mb_motors_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    // apply voltage and air pressure compensation
    const float compensation_gain = mb_motors_get_compensation_gain(motors); // compensation for battery voltage and altitude

    // pitch thrust input value, +/- 1.0
    const float roll_thrust = (motors->_roll_in + motors->_roll_in_ff) * compensation_gain;

    // pitch thrust input value, +/- 1.0
    const float pitch_thrust = (motors->_pitch_in + motors->_pitch_in_ff) * compensation_gain;

    // yaw thrust input value, +/- 1.0
    float yaw_thrust = (motors->_yaw_in + motors->_yaw_in_ff) * compensation_gain;

    // throttle thrust input value, 0.0 - 1.0
    float throttle_thrust = mb_motors_get_throttle(motors) * compensation_gain;

    // throttle thrust average maximum value, 0.0 - 1.0
    float throttle_avg_max = motors->_throttle_avg_max * compensation_gain;

    // throttle thrust maximum value, 0.0 - 1.0, If thrust boost is active then do not limit maximum thrust
    const float throttle_thrust_max = boost_ratio(motors, 1.0f, motors_mc->_throttle_thrust_max * compensation_gain);

    // sanity check throttle is above zero and below current limited throttle
    if (throttle_thrust <= 0.0f) {
        throttle_thrust = 0.0f;
        motors->limit.throttle_lower = true;
    }
    if (throttle_thrust >= throttle_thrust_max) {
        throttle_thrust = throttle_thrust_max;
        motors->limit.throttle_upper = true;
    }

    // ensure that throttle_avg_max is between the input throttle and the maximum throttle
    throttle_avg_max = math_constrain_float(throttle_avg_max, throttle_thrust, throttle_thrust_max);

    // throttle providing maximum roll, pitch and yaw range
    // calculate the highest allowed average thrust that will provide maximum control range
    float throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);

    // calculate throttle that gives most possible room for yaw which is the lower of:
    //      1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
    //      2. the higher of:
    //            a) the pilot's throttle input
    //            b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
    //      Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
    //      Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
    //      We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
    //      We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low.  We favor reducing throttle instead of better yaw control because the pilot has commanded it

    // Under the motor lost condition we remove the highest motor output from our calculations and let that motor go greater than 1.0
    // To ensure control and maximum righting performance Hex and Octo have some optimal settings that should be used
    // Y6               : MOT_YAW_HEADROOM = 350, ATC_RAT_RLL_IMAX = 1.0,   ATC_RAT_PIT_IMAX = 1.0,   ATC_RAT_YAW_IMAX = 0.5
    // Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.375, ATC_RAT_PIT_IMAX = 0.375, ATC_RAT_YAW_IMAX = 0.375
    // Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.75,  ATC_RAT_PIT_IMAX = 0.75,  ATC_RAT_YAW_IMAX = 0.375
    // Usable minimums below may result in attitude offsets when motors are lost. Hex aircraft are only marginal and must be handles with care
    // Hex              : MOT_YAW_HEADROOM = 0,   ATC_RAT_RLL_IMAX = 1.0,   ATC_RAT_PIT_IMAX = 1.0,   ATC_RAT_YAW_IMAX = 0.5
    // Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.25,  ATC_RAT_PIT_IMAX = 0.25,  ATC_RAT_YAW_IMAX = 0.25
    // Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.5,   ATC_RAT_PIT_IMAX = 0.5,   ATC_RAT_YAW_IMAX = 0.25
    // Quads cannot make use of motor loss handling because it doesn't have enough degrees of freedom.

    // calculate amount of yaw we can fit into the throttle range
    // this is always equal to or less than the requested yaw from the pilot or rate controller
    float yaw_allowed = 1.0f; // amount of yaw we can fit in
    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            // calculate the thrust outputs for roll and pitch
            motors_mat->_thrust_rpyt_out[i] = roll_thrust * motors_mat->_roll_factor[i] + pitch_thrust * motors_mat->_pitch_factor[i];

            // Check the maximum yaw control that can be used on this channel
            // Exclude any lost motors if thrust boost is enabled
            if (!math_flt_zero(motors_mat->_yaw_factor[i]) && (!motors->_thrust_boost || i != motors_mat->_motor_lost_index)) {
                const float thrust_rp_best_throttle = throttle_thrust_best_rpy + motors_mat->_thrust_rpyt_out[i];
                float motor_room;
                if (math_flt_positive(yaw_thrust * motors_mat->_yaw_factor[i])) {
                    // room to upper limit
                    motor_room = 1.0 - thrust_rp_best_throttle;
                } else {
                    // room to lower limit
                    motor_room = thrust_rp_best_throttle;
                }
                const float motor_yaw_allowed = MAX(motor_room, 0.0)/fabsf(motors_mat->_yaw_factor[i]);
                yaw_allowed = MIN(yaw_allowed, motor_yaw_allowed);
            }
        }
    }

    // calculate the maximum yaw control that can be used
    // todo: make _yaw_headroom 0 to 1
    float yaw_allowed_min = (float)motors_mc->_yaw_headroom * 0.001f;

    // increase yaw headroom to 50% if thrust boost enabled
    yaw_allowed_min = boost_ratio(motors, 0.5f, yaw_allowed_min);

    // Let yaw access minimum amount of head room
    yaw_allowed = MAX(yaw_allowed, yaw_allowed_min);

    // Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
    if (motors->_thrust_boost && motors_mc->motor_enabled[motors_mat->_motor_lost_index]) {
        // Check the maximum yaw control that can be used on this channel
        // Exclude any lost motors if thrust boost is enabled
        if (!math_flt_zero(motors_mat->_yaw_factor[motors_mat->_motor_lost_index])){
            const float thrust_rp_best_throttle = throttle_thrust_best_rpy + motors_mat->_thrust_rpyt_out[motors_mat->_motor_lost_index];
            float motor_room;
            if (math_flt_positive(yaw_thrust * motors_mat->_yaw_factor[motors_mat->_motor_lost_index])) {
                motor_room = 1.0 - thrust_rp_best_throttle;
            } else {
                motor_room = thrust_rp_best_throttle;
            }
            const float motor_yaw_allowed = MAX(motor_room, 0.0)/fabsf(motors_mat->_yaw_factor[motors_mat->_motor_lost_index]);
            yaw_allowed = boost_ratio(motors, yaw_allowed, MIN(yaw_allowed, motor_yaw_allowed));
        }
    }

    if (fabsf(yaw_thrust) > yaw_allowed) {
        // not all commanded yaw can be used
        yaw_thrust = math_constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
        motors->limit.yaw = true;
    }

    // add yaw control to thrust outputs
    float rpy_low = 1.0f;   // lowest thrust value
    float rpy_high = -1.0f; // highest thrust value
    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            motors_mat->_thrust_rpyt_out[i] = motors_mat->_thrust_rpyt_out[i] + yaw_thrust * motors_mat->_yaw_factor[i];

            // record lowest roll + pitch + yaw command
            if (motors_mat->_thrust_rpyt_out[i] < rpy_low) {
                rpy_low = motors_mat->_thrust_rpyt_out[i];
            }
            // record highest roll + pitch + yaw command
            // Exclude any lost motors if thrust boost is enabled
            if (motors_mat->_thrust_rpyt_out[i] > rpy_high && (!motors->_thrust_boost || i != motors_mat->_motor_lost_index)) {
                rpy_high = motors_mat->_thrust_rpyt_out[i];
            }
        }
    }
    // Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation
    if (motors->_thrust_boost) {
        // record highest roll + pitch + yaw command
        if (motors_mat->_thrust_rpyt_out[motors_mat->_motor_lost_index] > rpy_high && motors_mc->motor_enabled[motors_mat->_motor_lost_index]) {
            rpy_high = boost_ratio(motors, rpy_high, motors_mat->_thrust_rpyt_out[motors_mat->_motor_lost_index]);
        }
    }

    // calculate any scaling needed to make the combined thrust outputs fit within the output range
    float rpy_scale = 1.0f;
    if (rpy_high - rpy_low > 1.0f) {
        rpy_scale = 1.0f / (rpy_high - rpy_low);
    }
    if (throttle_avg_max + rpy_low < 0) {
        rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low);
    }

    // calculate how close the motors can come to the desired throttle
    rpy_high *= rpy_scale;
    rpy_low *= rpy_scale;
    throttle_thrust_best_rpy = -rpy_low;
    float thr_adj = throttle_thrust - throttle_thrust_best_rpy;
    if (rpy_scale < 1.0f) {
        // Full range is being used by roll, pitch, and yaw.
        motors->limit.roll = true;
        motors->limit.pitch = true;
        motors->limit.yaw = true;
        if (thr_adj > 0.0f) {
            motors->limit.throttle_upper = true;
        }
        thr_adj = 0.0f;
    } else if (thr_adj < 0.0f) {
        // Throttle can't be reduced to desired value
        // todo: add lower limit flag and ensure it is handled correctly in altitude controller
        thr_adj = 0.0f;
    } else if (thr_adj > 1.0f - (throttle_thrust_best_rpy + rpy_high)) {
        // Throttle can't be increased to desired value
        thr_adj = 1.0f - (throttle_thrust_best_rpy + rpy_high);
        motors->limit.throttle_upper = true;
    }

    // add scaled roll, pitch, constrained yaw and throttle for each motor
    const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj;
    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            motors_mat->_thrust_rpyt_out[i] = (throttle_thrust_best_plus_adj * motors_mat->_throttle_factor[i]) + (rpy_scale * motors_mat->_thrust_rpyt_out[i]);
        }
    }

    // determine throttle thrust for harmonic notch
    // compensation_gain can never be zero
    motors->_throttle_out = throttle_thrust_best_plus_adj / compensation_gain;

    // check for failed motor
    mb_motors_check_for_failed_motor(motors, throttle_thrust_best_plus_adj);
}

/// mb_motors_multicopter_ops

// output_to_motors - sends minimum values out to the motors
static void output_to_motors(mb_motors_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;
    mb_motors_mat_t motors_mat = (mb_motors_mat_t)motors;

    int8_t i;

    switch (motors->_spool_state) {
        case MB_MOTOR_SHUT_DOWN: {
            // no output
            for (i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
                if (motors_mc->motor_enabled[i]) {
                    motors_mc->_actuator[i] = 0.0f;
                }
            }
            break;
        }
        case MB_MOTOR_GROUND_IDLE:
            // sends output to motors when armed but not flying
            for (i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
                if (motors_mc->motor_enabled[i]) {
                    mb_motors_set_actuator_with_slew(motors, &motors_mc->_actuator[i], mb_motors_actuator_spin_up_to_ground_idle(motors));
                }
            }
            break;
        case MB_MOTOR_SPOOLING_UP:
        case MB_MOTOR_THROTTLE_UNLIMITED:
        case MB_MOTOR_SPOOLING_DOWN:
            // set motor output based on thrust requests
            for (i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
                if (motors_mc->motor_enabled[i]) {
                    mb_motors_set_actuator_with_slew(motors, &motors_mc->_actuator[i], mb_motors_thrust_to_actuator(motors, motors_mat->_thrust_rpyt_out[i]));
                }
            }
            break;
    }

    // convert output to PWM and send to each motor
    for (i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            mb_motors_rc_write(motors, i, mb_motors_output_to_pwm(motors, motors_mc->_actuator[i]));
        }
    }
}

/*
  disable the use of motor torque to control yaw. Used when an
  external mechanism such as vectoring is used for yaw control
*/
static void disable_yaw_torque(mb_motors_t motors)
{
    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        ((mb_motors_mat_t)motors)->_yaw_factor[i] = 0;
    }
}

/*
  call vehicle supplied thrust compensation if set. This allows
  vehicle code to compensate for vehicle specific motor arrangements
  such as tiltrotors or tiltwings
*/
static void thrust_compensation(mb_motors_t motors)
{
    if (((mb_motors_mc_t)motors)->_thrust_compensation_callback) {
        ((mb_motors_mc_t)motors)->_thrust_compensation_callback(((mb_motors_mat_t)motors)->_thrust_rpyt_out, MB_MOTORS_MAX_NUM_MOTORS);
    }
}

/*------------------------------------test------------------------------------*/


